#include "mainwindow.h"
#include <QMenuBar>
#include <QTimer>
#include <QToolBar>
#include "tool/parse_ini_file.h"

MainWindow::MainWindow(QWidget *parent) : wps_process_dock_ptr_(new WPSProcessDock(this)),  QMainWindow(parent)
{
    std::map<string, string> info;
    ParseIniFile pif;
    pif.ReadConfig("../config/parameter.ini", info, "coordinate");
    config_info_.basic_lat_ = atof(info["latitude"].c_str());
    config_info_.basic_lon_ = atof(info["longitude"].c_str());

    double origin[2] = {config_info_.basic_lat_, config_info_.basic_lon_};
    GPSCoordinateTool::Instance()->gps_linearize_init(origin);

    base_position_.lat = config_info_.basic_lat_;
    base_position_.lon = config_info_.basic_lon_;
    base_position_.global_x = 0;
    base_position_.global_y = 0;

    subscribe_topics_thread.reset(new std::thread(&vehicle::map::RunLcmSubscriber));
    subscribe_topics_thread->detach();
    publish_topics_thread.reset(new std::thread(&vehicle::map::RunLcmPublisher));
    publish_topics_thread->detach();

    addDockWidget(Qt::BottomDockWidgetArea, wps_process_dock_ptr_);
    wps_process_dock_ptr_->setVisible(false);
    wps_process_dock_ptr_->setFeatures(QDockWidget::NoDockWidgetFeatures);
    wps_process_dock_ptr_->setTitleBarWidget(new QWidget());

    setFocusPolicy(Qt::ClickFocus);
    map_widget_ = new Viewer2DWidget(this);
    setCentralWidget(map_widget_);

    setMinimumSize(800,600);
    creat_menu_bar();
    creat_tools_bar();
    creat_status_bar();

    timer1 = new QTimer(this);
    timer1->setInterval(15);
    connect(timer1, SIGNAL(timeout()), this, SLOT(loop()));
    timer1->start();

    OpenMapFileTriggered();
    initialization_ = true;
}


void MainWindow::loop()
{
    map_widget_->update();
}

MainWindow::~MainWindow()
{
    delete timer1;
}



void MainWindow::creat_menu_bar()
{
    menuBar = new QMenuBar(this);
    menuwindow = new QMenu("File", menuBar);
    QAction *save_map_file_action = new QAction("Save", this);
    connect(save_map_file_action, &QAction::triggered, this, &MainWindow::SaveMapInfoToMapFileTriggered);
    menuwindow->addAction(save_map_file_action);
    menuBar->addMenu(menuwindow);

    menu_mode_ = new QMenu("Mode", menuBar);
    single_action = new QAction("single mode", this);
    single_action->setCheckable(true);
    single_action->setChecked(true);
    connect(single_action, &QAction::triggered, this, &MainWindow::TrakingModeTriggered);
    multiple_action = new QAction("multiple mode", this);
    multiple_action->setCheckable(true);
    connect(multiple_action, &QAction::triggered, this, &MainWindow::TrakingModeTriggered);
    circle_action = new QAction("circle mode", this);
    circle_action->setCheckable(true);
    connect(circle_action, &QAction::triggered, this, &MainWindow::TrakingModeTriggered);
    menu_mode_->addAction(single_action);
    menu_mode_->addAction(multiple_action);
    menu_mode_->addAction(circle_action);
    menuBar->addMenu(menu_mode_);
    this->setMenuBar(menuBar);
}

void MainWindow::TrakingModeTriggered()
{
    single_action->setChecked(false);
    multiple_action->setChecked(false);
    circle_action->setChecked(false);
    QAction* action = qobject_cast<QAction*>(sender());
    if (action) {
        QString actionName = action->text();
        if(actionName.toStdString() == "single mode")
        {
            map_widget_->traking_mode_ = 0;
            single_action->setChecked(true);

        }else if(actionName.toStdString() == "multiple mode")
        {
            map_widget_->traking_mode_ = 1;
            multiple_action->setChecked(true);

        }else if(actionName.toStdString() == "circle mode")
        {
            map_widget_->traking_mode_ = 2;
            circle_action->setChecked(true);

        }
    }
}


void MainWindow::SaveMapInfoToMapFileTriggered()
{
    std::map<int, SetofMapInfo> map_info;
    string gps_prefix = "gps.txt.";
    string wps_prefix = "wps.txt.";
    map_info = vehicle::map::DataProviderMapInfoPoints::Instance()->map_info_points();
    if(map_info.size() > 0)
    {
        if(vehicle::tools::EnsureDirectory(operate_selected_directory_.toStdString()))
        {
            std::vector<string> file_lists = vehicle::tools::ListSubPaths(operate_selected_directory_.toStdString(),DT_REG);
            std::vector<string> result;
            for(auto file:file_lists)
            {
                if((file.find(gps_prefix) != std::string::npos)||(file.find(wps_prefix) != std::string::npos))
                {
                    result.push_back(file);
                }
            }
            for(auto set_of_map: map_info)
            {
                std::string gps_str = gps_prefix + std::to_string(set_of_map.first);
                std::ofstream gps_file;
                auto it = std::find(result.begin(),result.end(),gps_str);
                if(it != result.end())
                {
                    std::string file_path = operate_selected_directory_.toStdString() + "/" + gps_str;
                    if (unlink(file_path.c_str()) < 0) {
                        std::cout<<"remove: "<<file_path<<" failed"<<std::endl;
                    }
                    std::cout<<"save name"<<file_path<<std::endl;
                    gps_file.open(file_path);
                    gps_file<<"<LON>"<<"\t"<<"<LAT>"<<"\t"<<"<X/m>"<<"\t"<<"<Y/m>"<<"\t"<<"<YAW/rad>"<<"\t"<<"<STATUS>"<<std::endl;
                    for(int i=0;i<set_of_map.second.gps_points.size();i++)
                    {
                        gps_file << std::fixed << std::setprecision(7)<<set_of_map.second.gps_points.at(i).lon<<"\t"<<set_of_map.second.gps_points.at(i).lat<<"\t";
                        gps_file << std::fixed << std::setprecision(2)<<set_of_map.second.gps_points.at(i).global_x<<"\t"<<set_of_map.second.gps_points.at(i).global_y<<"\t";
                        gps_file << std::fixed << std::setprecision(2)<<set_of_map.second.gps_points.at(i).yaw<<"\t";
                        gps_file << std::fixed << std::setprecision(0)<<set_of_map.second.gps_points.at(i).location_state<<std::endl;
                    }
                }

                std::string wps_str = wps_prefix + std::to_string(set_of_map.first);
                std::ofstream wps_file;
                auto wps_it = std::find(result.begin(),result.end(),wps_str);
                if(wps_it != result.end())
                {
                    std::string file_path = operate_selected_directory_.toStdString() + "/" + wps_str;
                    if (unlink(file_path.c_str()) < 0) {
                        std::cout<<"remove: "<<file_path<<" failed"<<std::endl;
                    }
                    std::cout<<"save name"<<file_path<<std::endl;
                    wps_file.open(file_path);
                    wps_file<<"<SEQNUM>"<<"\t"<<"<LON>"<<"\t"<<"<LAT>"<<"\t"<<"<X>"<<"\t"<<"<Y>"<<"\t"<<"<SPEEDLIMIT>"<<"\t"<<"<LANE NUM>"<<"\t"<<"<LANE INDEX>"<<"\t"<<"<ROAD TYPE>"<<"\t"<<"<TURN TYPE>"<<"\t"<<"<LEFT BOUNDARY>"<<"\t"<<"<RIGHT BOUNDARY>"<<std::endl;
                    for(int i=0;i<set_of_map.second.wps_points.size();i++)
                    {
                        wps_file << std::fixed << std::setprecision(0)<<i<<"\t";
                        wps_file << std::fixed << std::setprecision(7)<<set_of_map.second.wps_points.at(i).lon<<"\t"<<set_of_map.second.wps_points.at(i).lat<<"\t";
                        wps_file << std::fixed << std::setprecision(2)<<set_of_map.second.wps_points.at(i).global_x<<"\t";
                        wps_file << std::fixed << std::setprecision(2)<<set_of_map.second.wps_points.at(i).global_y<<"\t";
                        wps_file << std::fixed << std::setprecision(2)<<set_of_map.second.wps_points.at(i).speed_limit<<"\t";
                        wps_file << std::fixed << std::setprecision(0)<<set_of_map.second.wps_points.at(i).lane_num<<"\t";
                        wps_file << std::fixed << std::setprecision(0)<<set_of_map.second.wps_points.at(i).lane_index<<"\t";
                        wps_file << std::fixed << std::setprecision(0)<<0<<"\t";
                        wps_file << std::fixed << std::setprecision(0)<<0<<"\t";
                        wps_file << std::fixed << std::setprecision(0)<<set_of_map.second.wps_points.at(i).left_bound<<"\t";
                        wps_file << std::fixed << std::setprecision(0)<<set_of_map.second.wps_points.at(i).right_bound<<std::endl;
                    }
                }
                gps_file.flush();
                gps_file.close();
                wps_file.flush();
                wps_file.close();
            }
        }
    }
}

void MainWindow::creat_tools_bar()
{
    editToolBar = new QToolBar("Main Toolbar", this);
    editToolBar->setVisible(true);

    open_map_  = new QAction(QIcon(":/images/ico/document_open.png"),tr("&open map"),this);
    editToolBar->addAction(open_map_);
    connect(open_map_,SIGNAL(triggered()),this,SLOT(OpenMapFileTriggered()));

    find_car_  = new QAction(QIcon(":/images/ico/car_address.png"),tr("&find_car"), this);
    editToolBar->addAction(find_car_);
    connect(find_car_,SIGNAL(triggered(bool)),map_widget_,SLOT(find_car_triggered()));

    record_gps_  = new QAction(QIcon(":/images/ico/record-map.png"),tr("&记录gps"),this);
    editToolBar->addAction(record_gps_);
    record_gps_->setCheckable(true);
    connect(record_gps_,SIGNAL(triggered()),this,SLOT(RecordGPSPointsTriggered()));

    record_wps_  = new QAction(QIcon(":/images/ico/road_edit.png"),tr("&place wps"),this);
    editToolBar->addAction(record_wps_);
    record_wps_->setEnabled(false);
    connect(record_wps_,SIGNAL(triggered()),this,SLOT(RecordWPSPointsTriggered()));

    edit_wps_  = new QAction(QIcon(":/images/ico/edit_road.png"),tr("&Edit WPS"),this);
    edit_wps_->setCheckable(true);
    editToolBar->addAction(edit_wps_);
    connect(edit_wps_,SIGNAL(triggered()),this,SLOT(EditWPSPointsTriggered()));


    edit_gps_  = new QAction(QIcon(":/images/ico/edit_gps.png"),tr("&Edit GPS"),this);
    edit_gps_->setCheckable(true);
    editToolBar->addAction(edit_gps_);
    connect(edit_gps_,SIGNAL(triggered()),this,SLOT(EditGPSPointsTriggered()));

    traking_gps_  = new QAction(QIcon(":/images/ico/traking.png"),tr("&traking GPS"),this);
    traking_gps_->setCheckable(true);
    editToolBar->addAction(traking_gps_);
    connect(traking_gps_,SIGNAL(triggered()),this,SLOT(TrakingGPSPointsTriggered()));

    measure_point_ = new QAction(QIcon(":/images/ico/measure.png"),tr("&measure point"),this);
    measure_point_->setCheckable(true);
    editToolBar->addAction(measure_point_);
    connect(measure_point_,SIGNAL(triggered()),this,SLOT(MeasurementPointsTriggered()));

    connect(this,SIGNAL(OpenMapFileListSignal(const std::string &,std::vector<std::string> &)),map_widget_,SLOT(OpenMapFileListTriggered(const std::string &,std::vector<std::string> &)));
    connect(map_widget_,SIGNAL(DisplayWPSPointInfoSignal(const STRoadPoint &,SetofMapInfo &)),wps_process_dock_ptr_,SLOT(OnDisplayWPSPointInfoTriggered(const STRoadPoint &,SetofMapInfo &)));
    connect(wps_process_dock_ptr_,SIGNAL(EmitEditWPSPointSignal(const STRoadPoint &,SetofMapInfo &)),map_widget_,SLOT(EditWPSPointTriggered(const STRoadPoint &,SetofMapInfo &)));

    this->addToolBar(editToolBar);
}

void MainWindow::creat_status_bar()
{

    locationLabel = new QLabel(this);
    mouseLable = new QLabel(this);
    timeLable = new QLabel(this);
    statusBar()->addPermanentWidget(locationLabel);
    statusBar()->addPermanentWidget(timeLable);
    statusBar()->addWidget(mouseLable);


}

void MainWindow::OpenMapFileTriggered()
{
    QString selectedDirectory;
    QString initialPath = QDir::currentPath();  // 当前路径
    initialPath = initialPath + "/..";
    if(initialization_)
    {
        // 获取用户选择的目录路径
        selectedDirectory = QFileDialog::getExistingDirectory(nullptr, "Select or Create Directory", initialPath);
        // 检查用户是否取消了选择操作
        if (selectedDirectory.isEmpty()) {
            std::cout<<"select empty file name"<<std::endl;
            return;
        }
    }else{
        selectedDirectory = initialPath + "/Default";
        vehicle::tools::EnsureDirectory(selectedDirectory.toStdString());
    }
    std::vector<string> file_lists = vehicle::tools::ListSubPaths(selectedDirectory.toStdString(),DT_REG);
    std::vector<string> result;
    for(auto file:file_lists)
    {
        if((file.find("gps.txt.") != std::string::npos)||(file.find("wps.txt.") != std::string::npos))
        {
            if(map_widget_->traking_mode_ == 1)
            {
                result.push_back(file);
            }else
            {
                if(file == "gps.txt.0" || file == "wps.txt.0")
                {
                    result.push_back(file);
                }
            }
        }
    }

    for(auto file:result)
        std::cout<<"load map file name : "<<selectedDirectory.toStdString()<<"/"<<file<<std::endl;
    operate_selected_directory_ = selectedDirectory;
    emit OpenMapFileListSignal(selectedDirectory.toStdString(),result);
}

void MainWindow::RecordWPSPointsTriggered()
{
    int value;
    if(sem_getvalue(&wps_semaphore_,&value) == 0)
    {
        sem_post(&wps_semaphore_);
    }
}

void MainWindow::EditGPSPointsTriggered()
{
    if(map_widget_->mouse_target_type_ == MouseTaskType::EDIT_GPS_POINTS)
    {
        map_widget_->mouse_target_type_ = MouseTaskType::UNKNOWN;
    }else{
        map_widget_->mouse_target_type_ = MouseTaskType::EDIT_GPS_POINTS;
    }
    UpdateActionChecked();
}


void MainWindow::TrakingGPSPointsTriggered()
{
    if(traking_gps_flag_ == MouseTaskType::TRAKING_GPS_POINTS)
    {
        traking_gps_flag_ = MouseTaskType::UNKNOWN;
    }else{
        traking_gps_flag_ = MouseTaskType::TRAKING_GPS_POINTS;
    }
}

void MainWindow::MeasurementPointsTriggered()
{
    if(map_widget_->measurement_point_flag_ == MouseTaskType::MEASUREMENT_POINTS)
    {
        map_widget_->measurement_point_flag_ = MouseTaskType::UNKNOWN;
    }else{
        map_widget_->measurement_point_flag_ = MouseTaskType::MEASUREMENT_POINTS;
    }
}

void MainWindow::EditWPSPointsTriggered()
{
    if(map_widget_->mouse_target_type_ == MouseTaskType::EDIT_WPS_POINTS)
    {
        map_widget_->mouse_target_type_ = MouseTaskType::UNKNOWN;
    }else{
        map_widget_->mouse_target_type_ = MouseTaskType::EDIT_WPS_POINTS;
    }
    UpdateActionChecked();
}


void MainWindow::RecordGPSPointsTriggered()
{
    if(map_widget_->mouse_target_type_ == MouseTaskType::RECORD_MAP_POINTS)
    {
        map_widget_->mouse_target_type_ = MouseTaskType::UNKNOWN;
        if(record_gps_thread_ && record_gps_thread_->joinable())
        {
            record_gps_thread_->join();
        }
        record_gps_thread_ = nullptr;
    }else{
        map_widget_->mouse_target_type_ = MouseTaskType::RECORD_MAP_POINTS;
        // 获取用户选择的目录路径
        QString initialPath = QDir::currentPath();  // 当前路径
        QString selectedDirectory = QFileDialog::getExistingDirectory(nullptr, "Select or Create Directory", initialPath);
        // 检查用户是否取消了选择操作
        if (selectedDirectory.isEmpty()) {
            std::cout<<"select empty file name"<<std::endl;
            return;
        }

        if(record_gps_thread_ == nullptr){
            record_gps_thread_ = new std::thread([this, selectedDirectory]() {
                RecordGPSProcessThread(selectedDirectory.toStdString());
            });
            record_gps_thread_->detach();
        }
    }

    UpdateActionChecked();
}

void MainWindow::UpdateActionChecked()
{
    record_gps_->setChecked(false);
    edit_wps_->setChecked(false);
    edit_gps_->setChecked(false);
    wps_process_dock_ptr_->setVisible(false);

    switch (map_widget_->mouse_target_type_){
        case MouseTaskType::UNKNOWN:
        setCursor(QCursor(Qt::ArrowCursor));
        edit_wps_->setEnabled(true);
        edit_gps_->setEnabled(true);
        record_wps_->setEnabled(false);
        break;
    case MouseTaskType::RECORD_MAP_POINTS:
        setCursor(QCursor(Qt::PointingHandCursor));
        record_gps_->setChecked(true);
        edit_wps_->setEnabled(false);
        edit_gps_->setEnabled(false);
        record_wps_->setEnabled(true);
        break;
    case MouseTaskType::EDIT_WPS_POINTS:
        setCursor(QCursor(Qt::PointingHandCursor));
        edit_wps_->setChecked(true);
        wps_process_dock_ptr_->setVisible(true);
        break;
    case MouseTaskType::EDIT_GPS_POINTS:
        setCursor(QCursor(Qt::PointingHandCursor));
        edit_gps_->setChecked(true);
        break;
    }
}

void MainWindow::RecordGPSProcessThread(const std::string &dir)
{
    std::string gps_file_name = dir + "/gps.txt.";
    std::string wps_file_name = dir + "/wps.txt.";

    sem_init(&wps_semaphore_,0,0);
    STRoadPoint last_point_info;
    std::string tomove_gps = gps_file_name + std::to_string(gps_file_num_-1);
    std::string tomove_wps = wps_file_name + std::to_string(gps_file_num_-1);
    if (vehicle::tools::fileExists(tomove_gps)) {
        if (unlink(tomove_gps.c_str()) < 0) {
            std::cout<<"ERROR! Unable to delete "<<tomove_gps<<std::endl;
        }
     }

    if (vehicle::tools::fileExists(tomove_wps)) {
        if (unlink(tomove_wps.c_str()) < 0) {
            std::cout<<"ERROR! Unable to delete "<<tomove_wps<<std::endl;
        }
     }

    // Rotate away any existing gps files
    for(int file_num = gps_file_num_-1; file_num>=0; file_num--) {
        std::string newname_gps = gps_file_name + std::to_string(file_num);
        std::string tomove_gps = gps_file_name + std::to_string(file_num-1);
        std::string newname_wps = wps_file_name + std::to_string(file_num);
        std::string tomove_wps = wps_file_name + std::to_string(file_num-1);
        if(vehicle::tools::fileExists(tomove_gps)) {
            if(0 != rename(tomove_gps.c_str(), newname_gps.c_str())) {
                std::cout<<"ERROR! ERROR!  Unable to rotate "<<tomove_gps<<std::endl;
            }
            if(vehicle::tools::fileExists(tomove_wps)) {
                if(0 != rename(tomove_wps.c_str(), newname_wps.c_str())) {
                    std::cout<<"ERROR! ERROR!  Unable to rotate "<<tomove_wps<<std::endl;
                }
            }
        }
    }

    std::string newname_gps = gps_file_name + std::to_string(0);
    gps_log_file_.open(newname_gps);
    std::cout<<"write gps file name:  "<<newname_gps<<std::endl;

    std::string newname_wps = wps_file_name + std::to_string(0);
    wps_log_file_.open(newname_wps);
    std::cout<<"write wps file name:  "<<newname_wps<<std::endl;

    gps_log_file_<<"<LON>"<<"\t"<<"<LAT>"<<"\t"<<"<X/m>"<<"\t"<<"<Y/m>"<<"\t"<<"<YAW/rad>"<<"\t"<<"<STATUS>"<<std::endl;
    wps_log_file_<<"<SEQNUM>"<<"\t"<<"<LON>"<<"\t"<<"<LAT>"<<"\t"<<"<X>"<<"\t"<<"<Y>"<<"\t"<<"<SPEEDLIMIT>"<<"\t"<<"<LANE NUM>"<<"\t"<<"<LANE INDEX>"<<"\t"<<"<ROAD TYPE>"<<"\t"<<"<TURN TYPE>"<<"\t"<<"<LEFT BOUNDARY>"<<"\t"<<"<RIGHT BOUNDARY>"<<std::endl;
    int wps_seqnum = 0;
    int point_state = 0;
    first_time_write_ = false;
    write_gps_points_.clear();
    write_wps_points_.clear();

    STRoadPoint point_info_l;
    while(map_widget_->mouse_target_type_ == MouseTaskType::RECORD_MAP_POINTS)
    {
       point_info_l = vehicle::map::DataProviderEgoPosition::Instance()->ego_position();
        double point_info1[2],point_info2[2];
        point_info1[0] = point_info_l.global_x;
        point_info1[1] = point_info_l.global_y;
        point_info2[0] = last_point_info.global_x;
        point_info2[1] = last_point_info.global_y;
        bool write_gps_flag = false;
        bool write_wps_flag = false;
        point_state = 0;
        if(GPSCoordinateTool::Instance()->DistancePoint2Point(point_info1,point_info2)> 1||(std::fabs(last_point_info.yaw-point_info_l.yaw)>15*M_PI/180))
        {
            write_gps_flag = true;
            last_point_info = point_info_l;
        }
        if(sem_trywait(&wps_semaphore_) == 0)
        {
            write_wps_flag = true;
        }
        if(point_info_l.location_state !=0)
        {
            if(first_time_write_ == false)
            {
                write_wps_flag = true;
                point_state = 1;
                first_time_write_ = true;
            }
            WriteWPSOrGPSToFile(&wps_log_file_,&gps_log_file_,point_info_l,write_gps_flag,write_wps_flag,point_state,wps_seqnum);
        }
    }
    point_info_l = vehicle::map::DataProviderEgoPosition::Instance()->ego_position();
    if((point_info_l.location_state !=0))
    {
        point_state = 2;
        WriteWPSOrGPSToFile(&wps_log_file_,&gps_log_file_,point_info_l,true,true,point_state,wps_seqnum);
    }
    sem_destroy(&wps_semaphore_);
    gps_log_file_.flush();
    gps_log_file_.close();
    wps_log_file_.flush();
    wps_log_file_.close();
}


void MainWindow::WriteWPSOrGPSToFile(std::ofstream *wps_file,std::ofstream *gps_file,const STRoadPoint point,bool write_gps,bool write_wps,int &point_state,int &wps_seqnum)
{

    if(write_gps)
    {
        *gps_file << std::fixed << std::setprecision(7)<<point.lon<<"\t"<<point.lat<<"\t";
        *gps_file << std::fixed << std::setprecision(2)<<point.global_x<<"\t"<<point.global_y<<"\t";
        *gps_file << std::fixed << std::setprecision(2)<<point.yaw<<"\t";
        *gps_file << std::fixed << std::setprecision(0)<<point.location_state<<std::endl;
        write_gps_points_.push_back(point);

    }
    if(write_wps)
    {
        *wps_file << std::fixed << std::setprecision(0)<<wps_seqnum++<<"\t";
        *wps_file << std::fixed << std::setprecision(7)<<point.lon<<"\t"<<point.lat<<"\t";
        *wps_file << std::fixed << std::setprecision(2)<<point.global_x<<"\t";
        *wps_file << std::fixed << std::setprecision(2)<<point.global_y<<"\t";
        if(point_state == 2)
            *wps_file << std::fixed << std::setprecision(2)<<0<<"\t";
        else
            *wps_file << std::fixed << std::setprecision(2)<<5<<"\t";
        *wps_file << std::fixed << std::setprecision(0)<<5<<"\t";
        *wps_file << std::fixed << std::setprecision(0)<<2<<"\t";
        *wps_file << std::fixed << std::setprecision(0)<<0<<"\t";
        *wps_file << std::fixed << std::setprecision(0)<<0<<"\t";
        *wps_file << std::fixed << std::setprecision(0)<<-4<<"\t";
        *wps_file << std::fixed << std::setprecision(0)<<4<<std::endl;
        write_wps_points_.push_back(point);

    }
}

